﻿/*!
* @file BackgroundSubtractorRadialReachFilter.cpp
* @author 橋本敦史
* @date Last Change:2010/Jun/18.
*/
#include "MmplUtils.h"
#include "MmplVector.h"
#include "ImageBackgroundSubtractorRadialReachFilter.h"
namespace mmpl{
	namespace image{

		/*!
		* @brief デフォルトコンストラクタ
		*/
		BackgroundSubtractorRadialReachFilter::BackgroundSubtractorRadialReachFilter(unsigned char gaussian_threshold,double gaussian_sigma):gaussian_threshold(gaussian_threshold),gaussian_sigma(gaussian_sigma),isThresholdSet(false),isBGset(false){
			init();
		}

		/*!
		* @brief デストラクタ
		*/
		BackgroundSubtractorRadialReachFilter::~BackgroundSubtractorRadialReachFilter(){

		}

		/*!
		@brief 初期化処理
		*/
		void BackgroundSubtractorRadialReachFilter::init(){
			// 8方向を定義
			// 0 > go to left top
			direction[0][0] = -1;
			direction[0][1] = -1;
			// 0 > go to top
			direction[1][0] = 0;
			direction[1][1] = -1;
			// 0 > go to right top
			direction[2][0] = 1;
			direction[2][1] = -1;
			// 0 > go to right 
			direction[3][0] = 1;
			direction[3][1] = 0;
			// 0 > go to right bottom 
			direction[4][0] = 1;
			direction[4][1] = 1;
			// 0 > go to bottom 
			direction[5][0] = 0;
			direction[5][1] = 1;
			// 0 > go to left bottom 
			direction[6][0] = -1;
			direction[6][1] = 1;
			// 0 > go to left 
			direction[7][0] = -1;
			direction[7][1] = 0;
		}

		/*!
		@brief 背景画像差分を行う
		@param img 対象となる観測画像
		*/
		void BackgroundSubtractorRadialReachFilter::apply(const Image& img,BGSubtractedImage* oimg){
			*oimg = img;
			Image gray(img);

			assert(isBGset);
			assert(isThresholdSet);

			if(1!=gray.getChannels()){
				if(gray.getChannels()!=1)
					gray.setChannels(1);
				cvCvtColor(img.getIplImage(),gray.getIplImage(),CV_BGR2GRAY);
			}
			rrc = bg_bit_pattern;
			unsigned char bit_pattern;
			Color white(255,255,255),black(0,0,0);
			for(int y=0,i=0; y < bg_bit_pattern.getHeight(); y++){
				for(int x=0; x < bg_bit_pattern.getWidth(); x++,i++){
					bit_pattern = getForeBitPattern(x,y,gray,ref_points[i]);
					bit_pattern = calcBitDiff(
						bit_pattern,
						bg_bit_pattern.getColor(x,y)[0]
					);
					if(bit_pattern<bit_diff){
						// 違いが大きいのでrrcを255とする
						rrc.setColor(x,y,white);
					}
					else{
						rrc.setColor(x,y,black);
					}
				}
			}
			Image rcmask(rrc);
//			printHoge();
//			std::cerr << rrc.getWidth() << ", " << rcmask.getWidth() << std::endl;
			cvSmooth(rrc.getIplImage(),rcmask.getIplImage(),CV_GAUSSIAN,0,0,gaussian_sigma,0);
//			printHoge();
			cvThreshold(rcmask.getIplImage(),rcmask.getIplImage(),gaussian_threshold,255,CV_THRESH_BINARY);
			rcmask.setChannels(3);
			cvAnd(img.getIplImage(),rcmask.getIplImage(),oimg->getIplImage());
//			printHoge();
		}

		/*!
		@brief 背景画像からRadialReachCorrelationを計算するための情報を抽出する
		@param bg 背景画像
		*/
		void BackgroundSubtractorRadialReachFilter::setBGImage(const Image& bg){
			// bgが1channelかどうかを判定する
			Image gray(bg);
			if(1!=gray.getChannels()){
				if(gray.getChannels()!=1)
					gray.setChannels(1);
				cvCvtColor(bg.getIplImage(),gray.getIplImage(),CV_BGR2GRAY);
			}
			bg_bit_pattern = gray;
			ref_points.resize(bg_bit_pattern.getWidth()*bg_bit_pattern.getHeight());
			for(int y=0,i=0;y<gray.getHeight();y++){
				for(int x=0;x<gray.getWidth();x++,i++){
					unsigned char bit_pattern = searchAround( x,y,gray,&(ref_points[i]));
					/*			for(int d=0;d<8;d++)
					std::cerr << "ref_points[" << x << "," << y << "][" << d << "=" << ref_points[i][0].x << "," << ref_points[i][0].y << std::endl; 
					*/
					bg_bit_pattern.setColor(
						x,y,Color(bit_pattern,bit_pattern,bit_pattern));
				}
			}
			isBGset = true;
		}

		/*!
		* @brief 背景画素値の周辺を探索して差分が大きい点を探す
		* @param bgimage 対象となる背景画像
		* @param reach_length_vector 8方向へ伸ばす腕の長さを返すポインタ
		* @return 各方向に対する大小関係のbit列
		*/
		unsigned char BackgroundSubtractorRadialReachFilter::searchAround(int x,int y,const Image& bgimage,std::vector<CvPoint>* points){
			unsigned char bit_pattern = 0;
			int bg_luminance = bgimage.getColor(x,y)[0];
			points->resize(8);
			for(int d=0;d<8;d++){
				int diff=0;
				int _x=x,_y=y;
				do {
					diff = static_cast<int>(bgimage.getColor(_x,_y)[0]) - bg_luminance;
					_x += direction[d][0];
					_y += direction[d][1];
				} while(abs(diff)<enough_diff && bgimage.isIn(_x,_y));
				points->at(d).x = _x - direction[d][0];
				points->at(d).y = _y - direction[d][1];
				if(diff>=0){
					bit_pattern += (1 << d);
				}
			}


			return bit_pattern;
		}

		/*!
		@brief 前景画像の各画素に対する大小関係のBitPatternを得る
		*/
		unsigned char BackgroundSubtractorRadialReachFilter::getForeBitPattern(int x,int y,const Image& fgimage,const std::vector<CvPoint>& points)const{
			unsigned char bit_pattern = 0;
			int fg_luminance = fgimage.getColor(x,y)[0];
			for(int d=0;d<8;d++){
				if(static_cast<int>(fgimage.getColor(points[d].x,points[d].y)[0])
					- fg_luminance
					>=0){
						bit_pattern += (1 << d);
				}
			}


			return bit_pattern;
		}

		void BackgroundSubtractorRadialReachFilter::setThreshold(const Image& bg0,const Image& bg1,double sigma_enphasis, double over_detection_cost){
			double mean_diff = 0.0;
			double sigma = 0.0;
			if(over_detection_cost>1){
				over_detection_cost=1;
			}

			Image gray0(bg0);
			Image gray1(bg1);

			if(1!=gray0.getChannels()){
				if(gray0.getChannels()!=1)
					gray0.setChannels(1);
				cvCvtColor(bg0.getIplImage(),gray0.getIplImage(),CV_BGR2GRAY);
			}
			if(1!=gray1.getChannels()){
				if(gray1.getChannels()!=1)
					gray1.setChannels(1);
				cvCvtColor(bg1.getIplImage(),gray1.getIplImage(),CV_BGR2GRAY);
			}

			assert(gray0.getWidth()==gray1.getWidth());
			assert(gray0.getHeight()==gray1.getHeight());


			std::vector<int> diff(gray0.getHeight()*gray0.getWidth(),0);
			// ノイズの期待値を得る
			for(int y=0,i=0;y<gray0.getHeight();y++){
				double col_mean_diff=0;
				for(int x=0;x<gray0.getWidth();x++,i++){

					diff[i] = abs(
						static_cast<int>(gray0.getColor(x,y)[0])-
						static_cast<int>(gray1.getColor(x,y)[0]));
					col_mean_diff += diff[i];
				}
				mean_diff += col_mean_diff / gray0.getWidth();
			}
			mean_diff /= gray0.getHeight();
			// ノイズの分散を計算
			for(unsigned int i=0;i<diff.size();i++){
				sigma += pow(diff[i]-mean_diff,2.0);
			}
			sigma /= diff.size();
			enough_diff = static_cast<int>(sigma_enphasis * sqrt(sigma));

			setBGImage(gray0);
			// 背景同士のBitPatternのヒストグラムを取る
			MmplVector cnt;
			cnt.resize(9,0);
			unsigned char bit_pattern;
			for(int y=0,i=0; y < bg_bit_pattern.getHeight(); y++){
				for(int x=0; x < bg_bit_pattern.getWidth(); x++,i++){
					bit_pattern = getForeBitPattern(x,y,gray1,ref_points[i]); 
					bit_pattern = calcBitDiff(
						bit_pattern,
						bg_bit_pattern.getColor(x,y)[0]
					);
					cnt[bit_pattern]++;
				}
			}

			for(int i=0;i<9;i++){
				cnt[i]/=diff.size();
			}

			/* for debug */
			std::cerr << "Bit Diff with 2 Background Images" << std::endl;
			std::cerr << diff.size() << std::endl;
			for(int i=0;i<9;i++){
				std::cerr << i << ": "<< cnt[i] << std::endl;
			}
			/* fot debug end */

			// 前景と背景はランダムに符号の一致、不一致が決まると仮定する
			MmplVector bernoulli;
			bernoulli.resize(9,0.0);
			for(int i=0;i<9;i++){
				// 8Cu * (1/2)^u * (1/2)^(1-u) = 8Cu/256
				bernoulli[i]= static_cast<double>(combination(8,i)) / 256;
			}

			// bernoulliは左右対象なことを利用して、エラー比率を計算し、閾値を決定
			double false_positive_ratio=0.0,false_negative_ratio=1.0;
			double cur_over_detection_ratio=0.0;
			bit_diff = 8;
			for(int i=0;i<9;i++){
				false_positive_ratio += cnt[i];
				false_negative_ratio -= bernoulli[i];
				cur_over_detection_ratio = 
					(1-over_detection_cost) * false_negative_ratio;
				if(cur_over_detection_ratio < over_detection_cost*false_positive_ratio){
					bit_diff = i;	
					break;
				}
			}
			isThresholdSet = true;
		}


		Image BackgroundSubtractorRadialReachFilter::getReffPixelDistribution(double* emphasis){
			Image dist(bg_bit_pattern);
			double max = 0.0;
			MmplVector cnt;
			cnt.resize(ref_points.size(),0.0);
			int index;
			for(unsigned int i=0;i<ref_points.size();i++){
				for(int d=0;d<8;d++){
					index = dist.getWidth()*ref_points[i][d].y +ref_points[i][d].x;
					cnt[index]++;
					max = max>cnt[index]?max:cnt[index];
				}
			}
			*emphasis = max/255;
			for(int y=0,i=0;y<dist.getHeight();y++){
				for(int x=0;x<dist.getWidth();x++,i++){
					unsigned char val = static_cast<unsigned char>(cnt[i]/(*emphasis));
					dist.setColor(x,y,Color(val,val,val));
				}
			}
			return dist;
		}





		/*** Accessor ***/
		int BackgroundSubtractorRadialReachFilter::getEnoughDiffThreshold()const{
			return enough_diff;
		}
		int BackgroundSubtractorRadialReachFilter::getBitDiffThreshold()const{
			return bit_diff;
		}

		/*!
		* @brief gaussian_sigma に値をセットするAccessor
		* @param gaussian_sigma セットしたい値
		*/
		void BackgroundSubtractorRadialReachFilter::setGaussianSigma(double gaussian_sigma ){
			this->gaussian_sigma = gaussian_sigma;
		}

		/*!
		* @brief gaussian_sigma の値を取得する
		* @param gaussian_sigma gaussian_sigmaの値
		*/
		double BackgroundSubtractorRadialReachFilter::getGaussianSigma()const{
			return gaussian_sigma;
		}


		/*!
		* @brief gaussian_threshold に値をセットするAccessor
		* @param gaussian_threshold セットしたい値
		*/
		void BackgroundSubtractorRadialReachFilter::setGaussianThreshold(int gaussian_threshold ){
			this->gaussian_threshold = gaussian_threshold;
		}

		/*!
		* @brief gaussian_threshold の値を取得する
		* @param gaussian_threshold gaussian_thresholdの値
		*/
		int BackgroundSubtractorRadialReachFilter::getGaussianThreshold()const{
			return gaussian_threshold;
		}

		const Image* BackgroundSubtractorRadialReachFilter::getRRCImage()const{
			return &rrc;
		}


		unsigned char BackgroundSubtractorRadialReachFilter::calcBitDiff(unsigned char a,unsigned char b){
			unsigned char correlation = ~(a^b);
			//	std::cerr << "correlation: " << bitchar2string(correlation) << std::endl;
			unsigned char sum = 0;
			unsigned char flag = 1;

			for(int d=0;d<8;d++){
				//		std::cerr << "flag: " << bitchar2string(flag) << std::endl;
				//		std::cerr << "corr: " << bitchar2string(correlation) << std::endl;
				//		std::cerr << "and : " << bitchar2string(flag & correlation) << std::endl;
				if(static_cast<unsigned char>(flag & correlation) > 0){
					//printHoge();
					sum++;
				}
				flag <<= 1;
			}
			/*	std::cerr << "a: " << bitchar2string(a) << std::endl;
			std::cerr << "b: " << bitchar2string(b) << std::endl;
			std::cerr << "a,b : bit correlation : correlation" << (int)a << "," << (int)b << ": " << (int)sum <<": " << (int)correlation <<  std::endl;
			*/
			return sum;
		}

	} // namespace image
} // namespace mmpl


